#include "Protocol.h"
#include "Commands.h"

int commandBuffer[20];  // Should be much larger than any command will ever be
int currentIncomingID;
int currentOutgoingID = 0;
bool gConnected = 0;

uint8_t* oneMessageAgo = NULL;
int oneMessageAgoLength = 0;
uint8_t* twoMessagesAgo = NULL;
int twoMessagesAgoLength = 0;

void SendMessage(uint8_t* message, int length)
{
	int checksum = currentOutgoingID;
	for(int i = 0; i < length; i++)
	{
		checksum = (checksum + message[i]) % LIMIT_BYTE;
	}
	Serial.write(START_BYTE);
	Serial.write(currentOutgoingID);
	Serial.write(message, length);
	Serial.write(TERMINATOR_BYTE);
	Serial.write(checksum);

	if(twoMessagesAgo != NULL)
	{
		delete [] twoMessagesAgo;
	}
	twoMessagesAgo = oneMessageAgo;
	twoMessagesAgoLength = oneMessageAgoLength;
	oneMessageAgo = message;
	oneMessageAgoLength = length;

	currentOutgoingID = (currentOutgoingID + 1) % LIMIT_BYTE;
}

void SendAnalogInput(uint8_t pin, uint8_t input)
{
	uint8_t* message = new uint8_t[3];
	message[0] = AnalogInput;
	message[1] = pin;
	message[2] = input;
	SendMessage(message, 3);
}

void SendDigitalInput(uint8_t pin, uint8_t input)
{
	uint8_t* message = new uint8_t[3];
	message[0] = DigitalInput;
	message[1] = pin;
	message[2] = input;
	SendMessage(message, 3);
}

void SendResponse(FailCode response, int packetID)
{
	uint8_t* message = new uint8_t[3];
	message[0] = Response;
	message[1] = response;
	message[2] = packetID;
	SendMessage(message, 3);
}

void SendFeedWatchdog()
{
	uint8_t* message = new uint8_t[1];
	message[0] = WatchdogFeed;
	SendMessage(message, 1);
}

void SendNotification(NotificationCode notificationCode)
{
	uint8_t* message = new uint8_t[2];
	message[0] = Notification;
	message[1] = notificationCode;
	SendMessage(message, 2);
}

void SendRequestToResend(int packetID)
{
	uint8_t* message = new uint8_t[2];
	message[0] = RequestResendCommand;
	message[1] = packetID;
	SendMessage(message, 2);
}

void ResendMessage(int packetID)
{
	if(packetID - currentOutgoingID == 1 && oneMessageAgo != NULL)
	{
		SendMessage(oneMessageAgo, oneMessageAgoLength);
	}
	else if(packetID - currentOutgoingID == 2 && twoMessagesAgo != NULL)
	{
		SendMessage(twoMessagesAgo, twoMessagesAgoLength);
	}
}

void ReadCommand()
{
	while(Serial.read() != START_BYTE) ;
	int index = -1;
	int checksum = 0; // checksum does not include start and terminator bytes
	while (1)
	{
		index++;
		while(Serial.available() < 1) ;
		int incoming = Serial.read();
		if (incoming == TERMINATOR_BYTE)
			break;
		else if (index < 20)
		{
			commandBuffer[index] = incoming;
			checksum = (checksum + incoming) % LIMIT_BYTE;
		}
	}
	while(Serial.available() < 1) ;
	if (Serial.read() == checksum)
	{
		int packetID = commandBuffer[0];
		Command* c = GetCommand(commandBuffer[1]);
		if (IsConnected())
		{
			if(packetID != (currentIncomingID + 1) % LIMIT_BYTE)
			{
				if(packetID == (currentIncomingID + 2) % LIMIT_BYTE)
					SendRequestToResend((currentIncomingID + 1) % LIMIT_BYTE);
				else if(packetID == (currentIncomingID + 3) % LIMIT_BYTE)
				{
					SendRequestToResend((currentIncomingID + 1) % LIMIT_BYTE);
					SendRequestToResend((currentIncomingID + 2) % LIMIT_BYTE);
				}
				else
					ResetArduino();
			}
			currentIncomingID = packetID;
			if(c->MyCommandType == Invalid)
				SendResponse(InvalidCommand, packetID);
			else if(c->ArgumentsLength != index - 2)
				SendResponse(IncorrectNumberOfArguments, packetID);
			else
			{
				c->SetArguments(commandBuffer + 2);
				SendResponse(c->DoCommand(), packetID);
			}
		}
		else if(c->MyCommandType == Connect)
		{
			SendResponse(c->DoCommand(), packetID);
			currentIncomingID = packetID;
		}
		else
			SendResponse(NotConnected, packetID);
	}
	else
		SendNotification(NotificationChecksum);
}

bool IsConnected()
{
	return gConnected;
}

void SetConnected(bool connected)
{
	gConnected = connected;
}
